#ifndef EXTRACT_PEDESTRAIN_CROSSING_H
#define EXTRACT_PEDESTRAIN_CROSSING_H

#include <ros/ros.h>
#include <sensor_msgs/CompressedImage.h>
#include <sensor_msgs/CameraInfo.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <visualization_msgs/Marker.h>
#include <std_msgs/ColorRGBA.h>

class ExtractPerdestrianCrossing{
public:
    ExtractPerdestrianCrossing(ros::NodeHandle& n)
    {
        image_sub_ = n.subscribe("/usb_cam/image_raw/compressed",5,&ExtractPerdestrianCrossing::image_callback,this);
        image_info_sub_ = n.subscribe("/usb_cam/camera_info",5,&ExtractPerdestrianCrossing::camera_info_callback,this);
        corner_pub_ = n.advertise<visualization_msgs::Marker>("points",5);
        line_pub_ = n.advertise<visualization_msgs::Marker>("line",5);
        std::vector<int> low_bound,high_bound;
        n.getParam("PedestrainCrossingLowBound",low_bound);
        n.getParam("PedestrainCrossingHighBound",high_bound);
        n.getParam("k_thres",k_thres_);
        low_bound_ = cv::Scalar(low_bound[0],low_bound[1],low_bound[2]);
        high_bound_ = cv::Scalar(high_bound[0],high_bound[1],high_bound[2]);
        camera_height_ = 0.2;
    }

private:
    void image_callback(const sensor_msgs::CompressedImageConstPtr& msg)
    {
        try
        {
            cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(msg,sensor_msgs::image_encodings::BGR8);
            auto image = cv_ptr->image;
            extract_perdestrain_crossing(image);
            //std::cout<<"Image: "<<msg->width<<" x "<<msg->height<<std::endl;
        }
        catch (cv_bridge::Exception& e)
        {
            // ROS_ERROR("Could not convert from '%s' to 'bgr8'.", msg->encoding.c_str());
            // ROS_ERROR("Could not convert from '%s' to 'bgr8'.",msg->format.c_str());
        }
    }

    void preprocess(cv::Mat& image,cv::Mat& mask)   //图像预处理
    {
        cv::Rect rect(0,540,1920,540);
        image=  cv::Mat(image,rect);
        cv::cvtColor(image,image,cv::COLOR_BGR2HSV);
        cv::inRange(image,low_bound_,high_bound_,mask);
        cv::bitwise_not(mask,mask);
        auto kernel = cv::getStructuringElement(cv::MORPH_RECT, cv::Size(3,3),cv::Point(-1,-1));
        cv::dilate(mask,mask,kernel,cv::Point(-1,-1),1);    //膨胀  
        cv::erode(mask,mask,kernel);        //腐蚀
    }

    void get_corner_point(cv::Mat& mask,std::vector<cv::Point>& up_corner)
    {
        std::vector<std::vector<cv::Point>> contours;  
        std::vector<cv::Vec4i> hierarchy;  
        findContours(mask,contours,hierarchy,cv::RETR_TREE,cv::CHAIN_APPROX_NONE,cv::Point());  
        cv::Mat imageContours=cv::Mat::zeros(mask.size(),CV_8UC3);  
        cv::Mat Contours=cv::Mat::zeros(mask.size(),CV_8UC1);  //绘制  
        std::vector<cv::Vec4f> lines;
        std::vector<std::vector<cv::Point>> contours_poly(contours.size());//用于存放折线点集

        for(int i=0;i<contours.size();i++)  
        {  
            cv::approxPolyDP(cv::Mat(contours[i]), contours_poly[i], 5, true);
            if(contours_poly[i].size()<4||contours_poly[i].size()>7)
                continue;
                drawContours(imageContours, contours_poly, i, cv::Scalar(255,0,0), 1, 8);  //绘制
            std::vector<cv::Point> cad_point;    //对于每个轮廓只取y最小的两个点
            for(int j=0;j<contours_poly[i].size();j++)
            {
                auto k = calculate_ratio(contours_poly[i][j],contours_poly[i][(j+1)%contours_poly[i].size()]);
                if(abs(k)>k_thres_ && get_distance(contours_poly[i][j],contours_poly[i][(j+1)%contours_poly[i].size()])>40)
                {
                    if(contours_poly[i][j].y<contours_poly[i][(j+1)%contours_poly[i].size()].y)
                    {
                        set_point(cad_point,contours_poly[i][j]);
                    }
                    else
                    {
                        set_point(cad_point,contours_poly[i][(j+1)%contours_poly[i].size()]);
                    }
                }
            }
            if(cad_point.size() == 2)
            {
                up_corner.emplace_back(cad_point[0]);
                up_corner.emplace_back(cad_point[1]);
                cv::circle(imageContours,cad_point[0],3,cv::Scalar(255,255,0),3);
                cv::circle(imageContours,cad_point[1],3,cv::Scalar(255,255,0),3);
            }
        }
        cv::imshow("Contours Image",imageContours); //轮廓  
    }

    void extract_perdestrain_crossing(cv::Mat& image)
    {
        cv::Mat mask;
        preprocess(image,mask);
        std::vector<cv::Point> up_corner;
        get_corner_point(mask,up_corner);
        calculate_depth(up_corner);
        cv::waitKey(1);
    }

    cv::Vec4f calculate_depth(std::vector<cv::Point>& corner)
    {
        std::vector<geometry_msgs::Point> corners;
        for(int i=0;i<corner.size();i++)
        {
            corners.emplace_back(calculate_depth(corner[i]));
        }
        cv::Vec4f line;
        fitLineRansac(corners,line,10,0.05);
        visulize_corner(corners,line);
        cal_distance(line);
        return line;
    }

    void camera_info_callback(const sensor_msgs::CameraInfoConstPtr& msg)
    {
        if(camera_info.K[0] == 0.0)
        {
            camera_info = *msg;
            for(int i=0;i<camera_info.K.size();i++)
            {
                camera_matrix_(i/3,i%3) = camera_info.K[i];
            }
            std::cout<<camera_matrix_<<std::endl;
        }
    }

    void visulize_corner(std::vector<geometry_msgs::Point>& hit_vector,cv::Vec4f& line)
    {
        visualization_msgs::Marker marker;
        marker.header.frame_id = "/map";
        marker.id = 0;
        marker.action = visualization_msgs::Marker::ADD;
        marker.type = visualization_msgs::Marker::POINTS;
        marker.scale.x = 0.05;
        marker.scale.y = 0.05;
        std_msgs::ColorRGBA color;
        color.a = 1.0;
        color.g = 1.0;
        for(int i=0;i<hit_vector.size();i++)
        {
            marker.points.emplace_back(hit_vector[i]);
            marker.colors.emplace_back(color);
        }
        corner_pub_.publish(marker);

        marker.type = visualization_msgs::Marker::LINE_STRIP;
        marker.id = 1;
        color.r = 1.0;
        marker.points.clear();
        marker.colors.clear();
        marker.pose.orientation.w = 1.0;
        geometry_msgs::Point start,end;
        start.x = line[2];
        start.y = line[3];
        end.x = line[0]+line[2];
        end.y = line[1]+line[3];
        marker.points.emplace_back(start);
        marker.points.emplace_back(end);
        marker.color = color;
        line_pub_.publish(marker);
    }


    bool set_point(std::vector<cv::Point>& p_vector,cv::Point p)
    {
        if(p_vector.size()<2)
        {
            p_vector.emplace_back(p);
            return true;
        }
        else
        {
            if(p.y<p_vector[0].y)
            {
                p_vector[0] = p;
                return true;
            }
            else if(p.y<p_vector[1].y)
            {
                p_vector[1] = p;
                return true;
            }
        }
        return false;
    }

    void cal_distance(cv::Vec4f& line)   //计算相机和车道线的距离
    {
        cv::Point2f dp(line[0],line[1]);
        cv::Point2f p(line[2],line[3]);
        std::cout<<line<<std::endl;
        double distance = fabs(p.y*dp.x - p.x*dp.y)/sqrt(pow(dp.x,2)+pow(dp.y,2));
        std::cout<<distance<<std::endl;
    }

    void fitLineRansac(const std::vector<geometry_msgs::Point>& corners,
                    cv::Vec4f &line,
                    int iterations = 1000,
                    double sigma = 1.,
                    double k_min = -7.,
                    double k_max = 7.)
    {
        std::vector<cv::Point2f> points;
        for(int i=0;i<corners.size();i++)
        {
            points.emplace_back(cv::Point2f(corners[i].x,corners[i].y));
        }
        unsigned int n = points.size();

        if(n<2)
        {
            return;
        }

        cv::RNG rng;
        double bestScore = -1.;
        for(int k=0; k<iterations; k++)
        {
            int i1=0, i2=0;
            while(i1==i2)
            {
                i1 = rng(n);
                i2 = rng(n);
            }
            const cv::Point2f& p1 = points[i1];
            const cv::Point2f& p2 = points[i2];

            cv::Point2f dp = p2-p1;//直线的方向向量
            dp *= 1./norm(dp);
            double score = 0;

            if(dp.y/dp.x<=k_max && dp.y/dp.x>=k_min )
            {
                for(int i=0; i<n; i++)
                {
                    cv::Point2f v = points[i]-p1;
                    double d = v.y*dp.x - v.x*dp.y;//向量a与b叉乘/向量b的摸.||b||=1./norm(dp)
                    //score += exp(-0.5*d*d/(sigma*sigma));//误差定义方式的一种
                    if( fabs(d)<sigma )
                        score += 1;
                }
            }
            if(score > bestScore)
            {
                line = cv::Vec4f(dp.x, dp.y, p1.x, p1.y);
                bestScore = score;
            }
        }
    }


    geometry_msgs::Point calculate_depth(cv::Point& p_2d)
    {
        geometry_msgs::Point p;
        double z = -camera_height_;
        double x = (p_2d.x-camera_matrix_(0,2))/(p_2d.y+540-camera_matrix_(1,2))*camera_height_;
        double y = camera_matrix_(1,1)*camera_height_/(p_2d.y+540-camera_matrix_(1,2));
        p.x = x;
        p.y = y;
        p.z = z;
        return p;
    }

    double calculate_ratio(cv::Point& a,cv::Point& b)
    {
        if(abs(b.x-a.x) < 1e-3)
        {
            if(a.x == 0 || a.x == 1919)
                return 0;
            else
                return 999;
        }
        else
            return double(b.y-a.y)/(b.x-a.x);
    }

    double get_distance(cv::Point& a,cv::Point& b)
    {
        double distance = pow(a.x-b.x,2)+pow(a.y-b.y,2);
        return sqrt(distance);
    }

private:
    ros::NodeHandle nh;
    ros::Subscriber image_info_sub_;
    ros::Subscriber image_sub_;
    ros::Publisher corner_pub_;
    ros::Publisher line_pub_;
    sensor_msgs::CameraInfo camera_info;
    double camera_height_;
    Eigen::Matrix3d camera_matrix_;

    cv::Scalar low_bound_;
    cv::Scalar high_bound_;
    double k_thres_;
};


#endif